'''
    动作之服务端简述：
        接收到客户端的请求后链接进入执行回调函数，定义反馈和结果对象，执行过程将更新后的反馈对象压入堆栈，执行完毕后将结果对象返回

    动作的服务端编写：
        实例化对象并链接执行回调函数
        回调函数以客户端传入的目标对象 goal作为参数
        执行回调函数中声明自定义的 feedback 、result 对象，并返回 result对象
        更新完 feedback的各项属性后利用 goal.publish_feedback(feedback)将反馈压入堆栈
        执行结果存入rusult对象的属性中并返回
'''


import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from my_interfaces.action import MoveCircle
import time


class Action_Circle_Server(Node):
    def __init__(self, name):
        super().__init__(name)
        self.action_server = ActionServer(self, MoveCircle, "action_circle", self._execute_callback)    # 创建对象并声明回调函数
        self.get_logger().info(f"Node \"{name}\" is ready!")

    def _execute_callback(self, goal):                  # 回调函数以客户端发送的目标对象作为参数
        self.get_logger().info("Get task, ing...")
        feedback = MoveCircle.Feedback()                # 创建反馈对象

        finish_flag = False        # 完成标志
        for i in range(0, 361, 30):
            feedback.state = i          # 更新反馈属性
            self.get_logger().info("Publish state: %d" % feedback.state)
            goal.publish_feedback(feedback)             # 将反馈对象压入堆栈(将作为参数传给客户端回调函数)
            time.sleep(0.2)
            if i == 360:
                finish_flag = True

        result = MoveCircle.Result()                    # 创建结果对象
        if finish_flag == True:                    
            goal.succeed()                              # 任务目标已达成
            result.finish = True                        # 定义结果属性
        else:
            result.finish = False
        self.get_logger().info("{:=^20}".format(str(result.finish)))

        return result                                   # 将结果对象压入堆栈


def main(args=None):
    rclpy.init(args=args)
    node = Action_Circle_Server("action_circle_server")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()